/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.assets.ArmSwitchPair;
import edu.wpi.first.wpilibj.templates.assets.FlywheelAnalogSensor;
import edu.wpi.first.wpilibj.templates.commands.ExampleCommand;
import java.util.Vector;

/**
 *
 * @author Jarrod Risley
 */
public class ShooterSubsystem extends Subsystem {
    
    /**
     * Global Class Variables
     */
    private Victor flywheelVictor;
    private Victor topIndexerVictor;
    private Victor bottomIndexerVictor;
    private Victor stopOneVictor;
    
    private DigitalInput firstTopSwitch;
    private DigitalInput firstBottomSwitch;
    private DigitalInput secondTopSwitch;
    private DigitalInput secondBottomSwitch;
    private DigitalInput thirdTopSwitch;
    private DigitalInput thirdBottomSwitch;
    
    private ArmSwitchPair switches[] = new ArmSwitchPair[3];

    private FlywheelAnalogSensor rotationalSpeedSensor;
    
    private String subsystemName;
    private int discCount = 0;
    private int shooterDiscCount = 0;
    
    private static final int THRESHOLD = 300; // calibrate me!
    public static final int SHOOT = 1;
    public static final int STOP = 0;
    public static final int INTAKE = 2;
    public static final int REGURGITATE = -1;
    private static final double DEPLOY_STOP = 1;
    private static final double RETRACT_STOP = -1;
    
    private Thread switchThread;
    
    
    // CONSTRUCTOR //
    
    
    /**
     * Creates the ShooterSubsystem.
     * 
     * @param name The name of the subsystem.
     * @param sidecarNum The sidecar to which this subsystem is attached.
     * @param flywheelPort The flywheel PWM port.
     * @param topIndexerPort The Top Indexer PWM port.
     * @param bottomIndexerPort The Bottom Indexer PWM port.
     * @param stopOne The first stop port.
     * @param firstTopPort The switch at the very top of the shooter.
     * @param firstBottomPort The switch underneath the top one.
     * @param secondTopPort The second switch on the arm, from the top.
     * @param secondBottomPort The second switch that is below the first one....
     * @param thridTopPort The one...aw, screw it.
     * @param thirdBottomPort you get the point.
     */
    
    public ShooterSubsystem(String name, int sidecarNum, int flywheelPort,
            int topIndexerPort, int bottomIndexerPort, int stopOne,
            int firstTopPort, int firstBottomPort, int secondTopPort,
            int secondBottomPort, int thirdTopPort, int thirdBottomPort,
            int flywheelLightPort) {
        
        System.out.println("Creating ShooterSubsystem " + name + "...");
        subsystemName = name;
        
        flywheelVictor = new Victor(sidecarNum, flywheelPort);
        topIndexerVictor = new Victor(sidecarNum, topIndexerPort);
        bottomIndexerVictor = new Victor(sidecarNum, bottomIndexerPort);
        stopOneVictor = new Victor(sidecarNum, stopOne);

        switches[0] = new ArmSwitchPair(sidecarNum, firstTopPort, 
                firstBottomPort);
        
        switches[1] = new ArmSwitchPair(sidecarNum, secondTopPort, 
                secondBottomPort);
        
        switches[2] = new ArmSwitchPair(sidecarNum, thirdTopPort, 
                thirdBottomPort);
        
        
        rotationalSpeedSensor = new FlywheelAnalogSensor(flywheelLightPort);
        
        System.out.println(subsystemName + " created.");
        
        
    } // end ShooterSubsystem Constructor
    
    
    // METHODS //
    
    /**
     * Grabs the frequency from the rotationalSpeedSensor and determines if 
     * the speed of the flywheel is above the threshold. 
     * 
     * @return True if the speed of the flywheel is above the threshold.
     */
    public boolean isReady() {
        
        if (rotationalSpeedSensor.getFrequency() 
                > THRESHOLD)
            return true;
        
        return false;
        
    } // end isReady
    
    /**
     * Spins up the shooter.
     */
    
    public void spinUpShooter() {
        
        flywheelVictor.set(SHOOT);
        
    } // end spinUpShooter
    
    private void recoverFromShot() {
        
        double startTime = Timer.getFPGATimestamp();
        
        do {
            flywheelVictor.set(SHOOT);
        } while(startTime + 0.5 > Timer.getFPGATimestamp());
        
    } // end recoverFromShot
    
    public void testAnalogSensor() {
        
        flywheelVictor.set(0.4);
        
        System.out.println("Frequency is: " + rotationalSpeedSensor.getFrequency());
        
    } // end testAnalogSensor
    
    /**
     * Sucks up a frizbee.
     */
    
    public void activate(int direction) {
        
        switch (direction) {
            case SHOOT:
                indexerShoot();
                break;
            case INTAKE:
                ingest();
                break;
            case REGURGITATE:
                topIndexerVictor.set(REGURGITATE);
                bottomIndexerVictor.set(REGURGITATE);
                break;
                
        } // end switch
        
           
    } // end ingest
    
    public void ingest() {
        
        bottomIndexerVictor.set(SHOOT);

    } // end indexerAdvance
    
    public void moveUpOne() {
        
        topIndexerVictor.set(SHOOT);        
        
    } // end moveUpOne
    
    /**
     * Shoots the stored disks.
     */
    public void indexerShoot() {
            
        topIndexerVictor.set(SHOOT);
        bottomIndexerVictor.set(SHOOT);
        
    } // end indexerShoot
    
    public void indexerStop() {
        
        topIndexerVictor.set(STOP);
        bottomIndexerVictor.set(STOP);
        
    } // end indexerStop
    
    /**
     * Stops all rotational motors and deploys all the stops.
     */
    
    public void stop() {
        
        stopOneVictor.set(DEPLOY_STOP);

        
        flywheelVictor.set(STOP);
        bottomIndexerVictor.set(STOP);
        topIndexerVictor.set(STOP);
        
    } // end stop
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
//        setDefaultCommand(new ExampleCommand());
    } // end initDefaultCommand
    
    /**
     * Checks all sensors to see if we have an inverted frizbee.
     * @return True if a frizbee is inverted.
     */
    
    private boolean orientationInverted() {
        
        
        
        return false;
        
    } // end orientationInverted
    
    public boolean frizbeeToBeShotInverted() {
        
        if (orientationInverted() && firstTopSwitch.get()) 
            return true;
        
        return false;
        
    } // end isInverted
    
    public void manualTopIndexerDrive(double input) {
        
        topIndexerVictor.set(input);

    } // end manualShooterDrive
    
    public void manualBottomIndexerDrive(double input) {
        
        bottomIndexerVictor.set(-input);
        
    } // end manualBottomDexer
    
    /**
     * Checks the state of the specified switch.
     * 
     * @param switchNum Which switch in the array to check
     * @return 
     */
    public boolean checkSwitch(int switchNum) {
        return switches[switchNum].getInput();
    } // end checkSwitch
    
    /**
     * Checks the state of the specified switch.
     * 
     * @param switchNum Which switch in the array to check.
     * @return 
     */
    public boolean checkTopSwitch(int switchNum) {
        return switches[switchNum].getTopInput();
    } // end checkTopSwitch
    
    /**
     * Increments the disc count...duh! =P
     */
    public void incrementDiscCount() {
        
        discCount++;
        System.out.println("Disc count is now at: " + discCount);
        
    } // end incrementDiscCount
    
    /**
     * Decrements the disk count.
     */
    public void decrementDiscCount() {
        
        discCount--;
        System.out.println("Disc Count is now at: " + discCount);
        
    } // end decrementDiscCount
    
    /**
     * Gets the disc count.
     * 
     * @return The disc count.
     */
    public int getDiscCount() {
        return discCount;
    } // end getDiscCount
    
    /**
     * Increments the number of discs currently in the shooter.
     */
    public void incrementShooterCount() {
        
        shooterDiscCount++;
        System.out.println("Shooter Disc Count is now at; " + shooterDiscCount);
        
    } // end incrementShooterCount
    
    /**
     * Decrements the shooter count... duh =P
     */
    public void decrementShooterCount() {
        
        shooterDiscCount--;
        System.out.println("Shooter Disc Count is now at: " + shooterDiscCount);
        
    } // end decrementShooterCount
    
    /**
     * Gets the number of discs currently in the shooter.
     * 
     * @return How many discs have hit the second switch pair.
     */
    
    public int getShooterCount() {
        return shooterDiscCount;
    } // end getShooterCount
    
    /**
     * Sets the indexer to spit out the discs.
     */
    public void reverseIndexer() {
        
        topIndexerVictor.set(REGURGITATE);
        bottomIndexerVictor.set(REGURGITATE);
        flywheelVictor.set(-1);
        
    } // end reverseIndexer
    
    /**
     * Gets the requested state from the specified switch.
     * 
     * @param switchNum Which switch in the array to look at.
     * @param state Which state to test for - use the ones in ArmSwitchPair!
     */
    public boolean getRequestedSwitchSequence(int switchNum, boolean state) {
        
        return switches[switchNum].didChangeRequestedState(state);
        
    } // end getRequestedSwitchSequence
    
    public boolean getFallingEdge(int switchNum, boolean oldState) {
        return switches[switchNum].fallingEdge(oldState);
    } // end getFallingEdge
    
    public boolean getRisingEdge(int switchNum, boolean oldState) {
        return switches[switchNum].risingEdge(oldState);
    } // end getRisingEdge
    
} // end ShooterSubsystem
